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Abstract 


A  technique  was  developed  for  estimation  of  launch 
vehicle  performance  parameters.  This  technique  used  an 
inverse  covariance  or  Bayes  filter.  Both  a  seven  state  and 
an  eight  state  dynamics  model  were  implemented  and  their 
performance  investigated.  Observations  consisted  of  angular 
infrared  measurements  from  two  orbital  sensors.  The  seven 
state  filter  had  3  position,  3  velocity  and  an  acceleration 
component  for  its  state  vector.  The  acceleration  state  was 
modelled  as  constant  between  measurement  updates.  After  the 
addition  of  a  fading  memory,  the  seven  state  filter  showed 
good  performance  in  estimating  a  variable  acceleration  pro¬ 
file.  The  eight  state  filter  had  3  states  each  for  position 
and  velocity,  and  seventh  and  eighth  states  involving  engine 
exit  velocity  and  propellant  mass  flow  rate.  Although  the 
eight  state  filter  had  a  better  model  for  the  acceleration, 
the  filter  proved  to  be  unsuccessful  in  its  estimation 


attempts . 


I  Introduction 


The  objective  of  this  research  was  to  develop  space  - 
based  procedures  to  estimate  performance  parameters  of 
launch  vehio'os.  In  the  past,  estimation  techniques  of 
position,  velocity  and  performance  parameters  of  launch  and 
reentry  vehicles  have  been  developed  using  ground-based 
tracking  radars  as  the  iata  source.  The  measurements  avail¬ 
able  wc  'Id  include  range,  range  rate,  azimuth  and  elevation. 

Ha\ ■ nc  i  space  reconnaissance  capability  would  allow 
for  a  u~'4  r  arce  of  observation  data.  Angular  observations 
of  azimuth  and  elevation  from  satellites  with  passive  infrared 
sensors  fall  into  this  category.  This  information  could  be 
used  in  conjunction  with  ground  based  data  or  give  valuable 
data  which  was  unattainable  before  due  to  the  lack  of  ground 
based  stations. 

This  paper  addresses  the  development  of  an  estimator 
using  these  satellite  observations  as  data.  An  inverse  co- 
variance  or  Bayes  filter  was  used  in  the  development  of  the 
estimator.  Because  no  range  measurements  are  available  from 
the  IR  sensors,  there  may  be  degradation  in  the  observability 
of  the  states.  Of  particular  interest  was  the  ability  of  the 
filter  to  estimate  the  accelerations  of  a  launch  vehicle  as 
well  as  its  position  and  velocity. 

For  the  problem,  both  a  seven-state  and  then  an  eight- 
state  filter  model  were  evaluated  for  their  performance  capa¬ 
bilities.  Also,  two  satellite  observers  were  used  for  data 
in  order  to  improve  observability  in  the  states. 


Assumptions: 

(1)  The  data  satellites  were  assumed  to  be  in  geosynchronous 
equatorial  orbits. 

(2)  The  accuracy  of  the  infrared  sensors  was  considered  to 

be  the  same  for  both  elevation  and  azimuth  angle  determination. 

(3)  The  acceleration  of  the  launch  vehicle  was  assumed  to  act 
along  the-  velocity  vector. 

(4)  A  spherical  earth  was  assumed  in  the  filter  model. 

Sequence  of  Presentation: 

The  derivation  of  the  dynamics  equations  of  the  filter 
are  presented  in  Chapter  II.  Both  seven-state  and  eight-state 
models  are  given.  Chapter  III  shows  the  development  of  the 
observation  relationships  between  the  heat  emitting  ballistic 
missile  and  the  satellite-based  infrared  sensor.  This  is 
followed  by  the  filter  development  in  Chapter  IV.  Included 
in  the  discussion  is  the  development  of  the  filter  equations 
and  the  Bayes  filter  algorithm.  The  testing  and  results  aie 
discussed  in  Chapter  V.  Finally,  Chapter  VI  contains  con¬ 
clusions  resulting  from  the  development  and  presents  recom¬ 
mendations  for  further  study. 


II  Problem  Dynamics 


In  the  development  of  dynamics  of  the  filter,  a  seven 
state  model  was  used  first.  The  states  included  three  for 
position,  three  for  velocity  and  one  for  acceleration  due  to  . 
thrust  or  drag.  Later,  an  eight  state  model  was  implemented 
to  see  if  improved  performance  of  the  filter  could  be  attained. 
In  this  case  the  acceleration  state  is  replaced  by  two  states, 
one  for  estimating  the  rocket  engine's  exit  velocity  and  the 
other  for  the  rocket's  relative  mass  flow  rate.  The  discus¬ 
sion  that  follows  will  first  address  the  development  of  the 

dynamics  equations  for  the  seven  state  model  and  will  conclude 
with  the  differences  needed  to  implement  the  eight  state  model. 

The  Seven  State  Model 

The  equations  of  motion  for  the  launch  vehicle  were 
derived  from  the  general  two-body  equation: 

F+1?=0  (2-1) 

r 

where  y  is  the  gravitational  parameter: 

y  =  GM  (2-2) 

A  rectangular  earth  inertial  coordinate  frame  was  used,  with 
the  z-direction  being  north.  So  r  was  defined  by, 

r  =  xi  +  yj  +  zk  (2-3) 

A  A  A 

where  i,  j,  and  k  are  unit  vectors  along  the  x,  y,  and  z 
axes  respectively.  The  acceleration  was  assumed  to  a^t  along 


the  velocity  vector  so: 


Adding  this  to  the  two  body  equation  gave: 


(2-4) 


r 


(2-5) 


For  the  seven-state  model  the  state  vector  was  defined 


by: 


x 


a 


(2-6) 


The  system  was  nonlinear,  so  the  state  vector  was  propagated 
in  time  by  use  of  the  differential  equation: 

x(t)  =  F  (x  (t)  ,  t)  (2-7) 

where  x  is  the  state  vector  at  time  t  and  F  is  a  vector  of 
nonlinear  functions  of  the  variables  of  which  x  is  comprised 
and  possibly  of  t  as  well. 

The  three  position  states  are  denoted  by  x,  y,  and  z 
The  time  rates  of  change  of  these  states  are  given  by  the_r 


respective  velocities: 


• 

X 

(2-7a) 

• 

y 

=  V 

y 

(2-7b) 

• 

z 

vz 

(2-7c) 

For  the  velocity  states/  the  time  rates  of  change  were 
derived  by  breaking  up  eg  2-5  into  the  respective  i,  j,  and 
k  components. 

v  =  -  x  +  a— ~  (2-8a) 
X  r  lv| 


(2-8b) 


z  +  a- 


v 


(2-8c) 


The  seventh  state,  acceleration,  was  me  ielled  as  being 
constant  over  the  time  intervals  between  data  updates. 
Ideally,  this  would  give  the  model  the  ability  to  estimate 
the  deceleration  of  a  reentry  vehicle  as  well  as  acceleration 
of  a  launch  vehicle.  So  the  rate  of  change  of  acceleration 
was  given  oy: 

a  =  0  (2-9) 

Then  forming  x  by  use  of  eqns  2-7,  2-8,  and  2-9,  gave  the  F 


vector  as: 


For  the  treatment  of  nonlinear  systems,  we  assume  the 
availability  of  a  nominal  trajectory,  xQ(t) ,  with  a  given  set 
of  initial  conditions  xo(0).  The  true  initial  conditions 
differ  slightly  from  the  assumed  initial  conditions  by  an 
unknown  amount,  5x(t) .  Assume  that  the  true  dynamics  solu¬ 
tion  can  be  represented  by: 

x(t)  =  xQ(t)  +  <5x  (t)  (2-11) 

Differentiation  of  eq  2-11  gives  us: 

x(t)  =  xQ(t)  +  6x(t)  (2-12) 

which  combining  with  eq  (2-7)  yields: 

xQ(t)  +  Sx(t)  =  F (xq (t)  +  Sx(t)  ,  t)  (2-13) 

By  applying  Taylor's  theorem  and  expanding  the  right 


side  of  eq  2-13  we  can  obtain: 


6x(t)  +  H.O.T. 


XQ(t)  +  $x(t)  =  F(XQ(t),  t)  +  A (t) 

x  (t) 
o 


(2-14) 


where  A(t)  is  a  matrix  found  by  taking  the  partial  derivatives 
of  the  F  vector  with  respect  to  the  state  variables,  evaluated 
at  the  nominal  solution,  xQ(t). 

A ( t)  =  21  (2-15) 

*o(t)  8X  *o(t) 

With  the  assumption  that  5x  is  small,  the  nominal  tra¬ 
jectory  satisfies  the  dynamics  model: 

xQ(t)  =  F(x0(t),  t)  (2-16) 

Subtracting  this  from  eg  2-14  and  neglecting  higher  order 
terms,  we  see  that  to  first  order,  5x(t)  satisfies  the  time- 
varying  linear  differential  equation: 

6£(t)  =  A  (t)  Sx(t)  (2-17) 

xQ(t) 

The  derivation  of  the  A(t)  matrix  is  given  in  Appendix  A. 

The  system  given  by  eq  2-17  is  linear  and  time  dependent. 
Therefore,  the  variations  in  the  state  are  propagated  by  the 
state  transition  matrix,  <J>: 


7 


with  the  initial  conditions: 

*'i(t0/t0)  =  I  (2-20) 

where  I  is  the  identity  matrix. 

The  Eight-State  Model 

For  the  derivation  of  the  eight  state  model,  thrust  was 
assumed  to  be  constant  for  each  stage  of  the  launch  vehicle. 
The  dynamics  of  the  system  remained  basically  the  same  with 
only  the  F  vector  and  A(t)  matrix  changing.  With  thrust 
constant,  the  acceleration  can  be  given  by: 


a  =  v. 


(mQ  -  mt) 


(2-21) 


where : 

a  =  acceleration  due  to  thrust 

m  =  the  propellant  mass  flow  rate  of  the  rocket  engine 
r-iQ  =  the  original  mass  of  the  launch  vehicle  (including 
propellant) 
t  =  time 


By  using  eq  (2-21)  in  this  form,  we  would  need  three  new 

states — v  ,  m,  and  mQ.  However,  by  dividing  the  numerator 

and  denominator  by  m  .  the  acceleration  becomes: 

■*  o 


(2-22) 


8 


where  M  is  the  relative  mass  flow  rate.  The  two  additional 
states  become  vg  and  M.  The  state  vector  for  the  eight 
state  model  is : 


and  the  resulting  F  vector  becomes: 


The  changes  in  the  A(t)  matrix  are  given  in  Appendix  A. 


Both  state  models  were  investigated  in  the  implementa¬ 
tion  of  the  estimator.  Although  not  physically  correct,  the 
seven-state  model  was  attractive  because  of  its  general 
nature.  This  would  hopefully  allow  the  filter  to  adapt  to 
sudden  changes  in  acceleration,  such  as  those  experienced  in 
a  rocket  staging.  However,  with  the  acceleration 'assumed 
constant,  one  would  have  to  be  careful  on  how  many  data  points 
were  used  in  an  update.  If  the  change  in  acceleration  was 
great  in  a  data  span,  this  would  make  the  filter's  constant 
acceleration  approximation  less  valid.  The  eight  state  model 
has  a  better  physical  representation  of  the  acceleration  state. 
This  should  lend  to  batching  a  larger  amount  of  data  to  get  an 
improved  update.  But,  the  ability  of  the  filter  to  detect 
staging  may  be  degraded.  The  characteristics  of  each  model 
will  be  discussed  in  more  detail  later. 


Ill  Observation  Relationships 


Azimuth  and  Elevation  Derivation 

The  derivation  of  the  angular  relationships  for  the 
problem  was  obtained  from  Miller’s  presentation  (Ref  6). 

The  following  will  basically  follow  his  development  of  the 
observation  angles. 

The  measurements  from  the  observation  satellite  were  the 
two  angles,  azimuth  and  elevation,  depicted  in  Figure  1. 

These  angular  relationships  were  derived  with  the  observer 
and  launch  vehicle  in  the  same  rectangular  coordinate  frame. 
Azimuth  was  the  angle  (clockwise  direction  being  positive) 
between  a  local  vertical ,  z ’ ,  and  a  local  position  vector , 
r ' .  Elevation  was  the  angle  between  the  negative  of  the 
position  vector  of  the  observer,  -R,  and  the  vector  from  the 
observer  to  the  target  vehicle,  p“.  Both  angles  were  measured 
in  radians.  The  angular  relationships  were  derived  from  the 
equations 


cos  y 


a  »  b 
a|  |  b 


(3-1) 


sin  Y 


a  x  5| 

|a||b| 


(3-2) 


Elevation  is  the  angle  between  p  and  -R,  so  from  Figure 
2  and  eq  3-1  we  see: 


COS  <f)  = 


R  •  r 


(3-3) 
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position  vector  of  observer 
position  vector  of  launch  vehicle 

position  vector  of  launch  vehicle  relative  to  observer 


position  vector  (in  a  plane  orthogonal  to  R)  of  launch 
vehicle  relative  to  O' 

elevation  angle;  the  angle  subtended  by  p"  and  -R 

azimuth  angle;  the  angle  subtended  by  r*  and  the  line 
segment  from  O'  to  z ' 


Figure  1.  Illustration  of  observation  angles 


a  unit  vector  normal  to  R  and  containing  point  P 
a  unit  vector  along  R 


r|  cos  <p 


(3-8) 


t  =  r  - 


r| 


and  combining  with  the  results  of  eq  3-3  we  get: 


t  =  F-  -i.  |r| 

R 


R  •  r 


R  r 


=  r  -  R 


R  *  r 


R 


From  Figure  2  we  see: 


sin(el)  = 


1IL 

IpI 


and: 


p  =  r  -  R 


(3-9) 


(3-l( 


(3-11 


Now  substituting  eqs  3-9  and  3-11  into  eq  3-10  we  get: 


sin(el)  = 

|r  -  R| 


(3-12! 


which  gives  us  the  relationship  for  the  elevation  angle  as: 


el  =  sin 

Azimuth  was  given  as  the  angle  between  a  local  vertical 
and  a  local  position  vector.  So  from  Figure  3  and  using 
eq  3-1  we  get: 


r  -  R 


1  A 


f’.V. 


A 


t  =  vector  the  same  as  in  Figure  2;  normal  to  R  and 
containing  point  P. 

A 

k  =  a  unit  vector  in  the  z  direction. 


Figure  3.  Geometry  for  azimuth  derivation 


cos (az) 


(3-14) 


and  substituting  eq  3-9  in  for  t  gives: 


(3-15) 


This  gives  us  the  relationship  for  the  azimuth  angle  as: 


From  eq  3-15,  the  sign  of  the  azimuth  angle  is  not  readily 
discernable.  So  to  determine  the  sign,  we  look  at  the  z  com¬ 
ponent  of  the  cross  product  of  R  x  r.  If  the  z  component  was 
negative,  then  the  azimuth  was  negative. 

Observation  Relation  Derivation 

The  relationships  of  the  angular  measurements,  azimuth 
and  elevation,  are  nonlinear  functions  of  the  state  vector. 

A  set  of  discrete  observations  are  related  to  the  state  vari¬ 
ables  by  the  general  nonlinear  relationship: 

z^)  =  G(x(t±)  ,tL)  (3-17) 

Evaluating  the  observation  vector  along  the  nominal  trajectory, 
xQ(t) ,  at  discrete  times,  t^,  yields  the  nominal  measurements: 

W  =  G (5cq (t±)  , t±)  (3-18) 

Now  the  true  observations  satisfy  the  equation: 


2(ti)  =  G(xo(ti)  +  Sxitr,  ,tL)  (3-19) 

By  expanding  this  equation  in  a  Taylor's  series  about  tne 
nominal  trajectory  yields: 


z  (t^  =  G(xQ(ti)  ,t£)  + 


9G(xo(ti} ,ti) 


9x ( t^) 


+  H.O.T. 


X  (t. ) 
o  1 


(3-20) 


subtracting  eq  3-18  from  both  sides  of  eq  3-20  and  neglecting 
higher  order  terms  gives  a  relationship  for  the  residuals  of 


y  v 


the  observations,  r(t^) — the  difference  between  the  true  and 
the  nominal  observations: 

Sx(t±)  (3-21a) 

y(ti> 

=  Hix^y)  ,ti)«x(ti)  (3-21b) 

where  H  is  the  partial  derivative  matrix  of  the  observation 
vector  with  respect  to  the  state  vector  evaluated  at  the 
nominal  trajectory. 

To  make  the  residual  the  difference  at  the  epoch,  t 
instead  of  t^,  the  state  transition  matrix  is  once  again  used: 

r(t.)  s  H(J0(t.),ti)$(ti,t0)«x(t0)  ( 3-22a) 

=  Tft^SxCt^  (3-22b) 

The  elements  of  the  H  matrix  are  derived  in  Appendix  B. 


F(t.)  S 


2(^1  - 


z  (t.)  = 
o  1'  3x 
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IV  Filter  Development 


This  chapter  shows  the  derivation  of  the  equations  for 
the  Bayes  filter  and  the  algorithm  for  implementation  of  the 
filter. 

The  Bayes  Filter 

From  Chapter  II,  the  problem  dynamics  were  given  by  the 
vector  equation: 

k  =  F(x,t)  (2-7) 

Also,  it  was  shown  that  with  the  dynamics  of  the  problem  well 
understood,  deviations  in  the  state  could  be  expressed  by: 

<$x(t)  =  $(t,t0)6x(tQ)  (2-18) 

as  long  as  the  Sx's  were  small. 

In  Chapter  III,  a  relationship  for  the  residual,  r(t^) , 
was  derived  by  finding  the  observation  relation  H(t^)  and 
using  eq  2-18  and  was  given  by: 

r(ti)  =  H(ti)l(ti,t0)5x(t0) 

=  T(ti)6x(t0)  (3-22) 

Due  to  corruptive  noise  in  the  measurements,  there  is  an 
error  in  the  residual  approximation.  This  is  shown  as: 

rft^  =  T(ti)6x(tQ)  +  e(t^)  (4-1) 

Define  as  the  observation  covariance  matrix,  containing 
information  about  the  accuracy  of  the*  data.  For  this  application 
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is  chosen  as  diagonal  under  the  conditions  that  the  random 
errors  in  azimuth  and  elevation  are  uncorrelated  within  a 
given  observation. 

Now,  using  Gaussian  error  statistics,  the  probability 
density  function  for  the  error  vector  is  expressed  as: 

f(e)  =  (2TT)”N/2jQr3sexp(-J5J)  (4-2) 

— T  -1— 
where  J  =  e  Q  e. 

To  maximize  the  probability,  the  principle  of  maximum  likeli¬ 
hood  is  used.  J,  the  weighted  least  squares  cost  function, 
must  be  minimized  in  order  that  f  be  a  maximum.  J  is  of  the 
form: 

J  =  [r(t.)  -  TU-Mx^n’oT^Fft.)  -  T(t.)SF(t0)]  (4-3) 

-1 

Since  is  positive  definite,  the  necessary  and  suffi¬ 
cient  condition  for  minimizing  J  is: 

— 2£ -  =  TJt  (4-4) 

8<5x(to) 

solving  for  <$x(tQ)  yields  (ref  5)  : 

<5x (tQ)  =  (T(ti)TQ71T(ti))"1T(ti)TQ71r(ti)  '  (4-5) 

With  <5x  assumed  as  zero  mean,  the  covariance  associated 
with  6x  is: 

P£(tQ)  =  E  (<Sx(t0)  Sx(t0)T)  (4-6) 
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T  —1  -1  T  — 1 

where  E  is  the  expectation  operator.  Let  W  =  (T  Q  T)  T  Q 
and  substituting  <5x  from  eq  4-5  gives: 

T 

P~ (t  )  =  E(W  r  r  WT)  (4-7) 

X  O  ~  11 

and  since  W  is  deterministic: 

P-(t  )  =  W  E(r  rT)  WT  (4-8) 

—X  O  —  ■“ 

—  — T 

But  the  expected  value  of  r  r  is  defined  as  the  covariance 
of  the  observations,  Q.  So  eq  4-8  can  be  written  as: 

P-(tQ)  =  WQWT  (4-9) 

Expanding  and  simplifying: 

P&(t  Q)  =  (ttq_1t)  (ttQ_1t) -W11t 

_  (  ttq-1t)  _1TT  (TV1)  T  [  (TTQ_1T)  “1]T 

P~(t  )  =  (T^-1!)”1  (4-10) 

—x  o  —  —  — 

Equations  4-5  and  4-10  are  used  in  basic  least  squares 
estimation.  A  sequential  manner  of  handling  the  data  can  be 
used  and  this  is  the  Bayes  filter.  This  is  done  by  combining 

A  ^ 

an  old  estimate,  x  (tQ) ,  and  its  covariance,  J>(t~)  ,  with  a 

new  batch  of  data  z.  x  (t  )  is  treated  as  data,  rather  than 

o 

reprocessing  all  tho  old  data  that  went  into  forming  the 
estimate.  So,  the  observation  relations  for  the  new  estimate 
are: 

x  =  I_  x  (tQ)  (4-11) 

z  =  G  ,ti)  .  (4-12) 
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The  augmented  matrices  become 


T  =  I 
— aug  — 


(4-13) 


Q  =  P(-)  0 

^aug  —  — 


(4-14) 


(x  -  xQ) 


(4-15) 


where  x  is  the  assumed  nominal  state, 
o 

With  this,  the  new  covariance  is: 


T  -1  -1 

(T  0  T  )  i 
vi-aug  saug  taug' 


=  (p_1(-)  +  tT  qT1  t.)'1 


(4-16) 


and  the  correction  to  the  state  vector  is: 


T  -1  -1  T  -1  — 

(T  Q  T  )  x  T  O  r 

-  aug  =aug  ^aug'  -aug  ^aug  -aug 


=  (P_1(~)  +  T?  QT1  T^"1  (P_1(-)  (x  -  xQ)  +  T l  QT1 


The  algorithm  below  shows  the  step  by  step  iterative 


process  used  to  converge  upon  a  solution. 


The  Bayes  Filter  Algorithm 


1 .  Input 


a.  Estimate  at  epoch 


1.  x(t0) 


21 


b.  New  Data 


5.  Compute  update  of  covariance  and  state: 

a.  P(+)  =  (P~1(t7)  +  TT  Q-1  T)"1 

—  —  1  t  -l 

b.  5x  =  P(+) (P_1(t7) (x(t^)  -  x0(t±))  +  T  Q  r) 

6.  Update  the  nominal  solution: 

*o(ti>  =  *o(ti}  + 


7.  Convergence  check.  If  convergence  criteria  is  met  then: 


If  not  met,  return  to  step  2  with  newly  computed  xQ (tQ)  and 
repeat  the  process. 

8.  Propagate  estimate  and  covariance  to  new  epoch  and  begin 
process  over  again.  Continue  until  launch  trajectory  is 
complete.  The  covariance  is  propagated  by: 

P(t.  + x>  =  iT(t.  +  +  ^ 


The  convergence  criteria  is  based  upon  the  true  solution 
being  the  result.  Theoretically  <5x  will  converge  to  zero. 
However,  in  practice,  it  should  be  allowed  to  converge  to 
within  the  associated  square  root  of  the  covariance  for  that 
element,  /p77.  The  residuals,  independently,  should  be  of 
order  v7qT7  as  they  converge. 

Although  no  estimator  is  completely  self  starting,  the 
Bayes  filter  can  be  started  with  only  a  guess  for  the  nominal 
state.  This  is  reflected  in  the  algorithm  by  initializing 
the  inverse  of  the  covariance  matrix,  P  ^(t~)  as  the  null 
matrix.  This  indicates  that  there  is  no  a  priori  knowledge 
of  the  system,  which  can  certainly  be  the  case  when  looking 
at  the  launch  data  of  a  missile.  In  this  case,  the  Bayes 
filter  reverts  to  a  least  squares  filter  and  the  first  update 
is  accomplished  looking  only  at  the  measurements. 


V  Testing  and  Results 


Setup  of  Observers 

As  mentioned  before,  two  observers  were  used  in  the 
problem,  assumed  to  be  in  geosynchronous  equatorial  orbit. 

This  stereo  view  would,  hopefully,  prevent  any  problems  in 
observability  that  might  arise  using  only  one  observer.  These 
problems  could  occur  if  launch  was  directly  below  the  observer 
or  the  launch  vehicle  was  traveling  away  from  the  observer. 

The  second  situation  is  displayed  in  Figure  4. 


R  =  observer  position  vector 
r  =  true  target  vehicle  position  vector 
r^_2  =  alternate  target  vehicle  position  vectors 
"p  =  vector  from  observer  to  target  vehicle 

Figure  4.  Measurement  ambiguity  with  one  observer 

This  shows  that  along  certain  trajectories  there  is  very 
little  change  in  the  azimuth  and  elevation  measurements. 
Therefore,  not  much  information  would  be  given  to  the  filter 


regarding  changes  in  the  states,  especially  velocity  and 


acceleration. 

The  tracking  sequences  were  initialized  with  the  two 
observers  positioned  90°  apart  along  the  same  orbit.  One 
was  positioned  on  the  -y  axis  and  the  other  on  the  x  axis  as 
shown  in  Figure  5.  This  caused  no  loss  in  generality  since 
the  coordinate  system  chosen  was  arbitrary  and  the  launch 
vehicle  was  initialized  from  a  point  that  was  not  on  any  of 
the  coordinate  axes.  During  tracking,  the  observers  pro¬ 
gressed  counterclockwise  along  their  orbit. 


Figure  5.  Initial  positions  of  target  and  observers 


Truth  Model 


The  truth  model  generated  the  true  azimuth  and  elevation 
data  at  given  times  during  the  launch  profile.  This  data  was 
generated  using  a  computer  program  that  propagated  the  states 
of  the  launch  vehicle  using  the  two  body  equation  of  motion. 
The  position  and  velocity  were  propagated  using  the  equations 
previously  developed  in  Chapter  II,  eqs  2-7  and  2-8.  To  model 
the  acceleration,  it  was  assumed  that  the  thrust  was  constant 
for  each  stage.  With  this,  the  acceleration  was  given  by  eq 
2-22.  Then  taking  the  time  derivative  gave: 

.  V  M2 

a  =  - ® T  (5-1) 

(1  -  Mt)  * 

which  was  used  in  the  truth  model  to  propagate  the  accelera¬ 
tion  state. 

The  position  states  were  then  combined  with  the  position 
vectors  of  each  observer  at  the  respective  times  and  substi¬ 
tuted  into  eqs  3-13  and  3-16  for  the  angular  measurements 
required  for  the  data. 

Values  for  the  thrust  profile  were  derived  from  para¬ 
meters  of  a  Titan  IIIB  rocket  (ref  8) . 

1st  Stage  -  8243.2  ft/sec 
Thrust  -  464,900  lb 

Propellant  mass  flow  rate  -  56.398  lb-sec/ft 
Initial  mass  -  11,275  lb-sec2/ft 

2nd  Stage  Vg  -  10,220.3  ft/sec 
Thrust  -  102,300  lb 

Propellant  mass  flow  rate  -  10.0095  Ib-sec/ft 


;>  ^tassL'. <lc  ‘M.:  -.v. 

*'*1  • 


O 


Initial  mass  -  2735  lb-sec  /ft 


3rd  Stage  Vg  -  9402.4  ft/sec 
Thrust  -  16,000  lb 

Propellant  mass  flow  rate  -  1.7017  Ib-sec/ft 

o 

Initial  mass  -  455  lb-sec  /ft 

Figure  6  shows  the  acceleration  profile  given  by  the  truth 
model . 


Figure  6.  Truth  model  acceleration  versus  time 


Computer  Program  Development 

The  first  step  in  developing  the  computer  program  for 

i 

the  filter  was  verification  of  the  partial  matrices  A  and  H 
that  were  developed  in  Chapters  II  and  III.  After  setting 
up  the  A  matrix  as  given  in  Appendix  A,  the  individual 
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elements  were  verified  by  conducting  a  numerical  check  given 


by: 


Fi(xj  +  <S,t) 
5“ 


(5-2) 


where : 

1.U  J.U 

A.  .  =  the  element  in  the  i  row  and  jtn  column 
iD 

6  =  a  deviation  on  the  order  of  10  or  smaller 

_ _  _ 

F^(x,t)  =  the  i  component  of  F ,  evaluated  at  x 

F^  (Xj  +  <S,t)  =  the  i^  component  of  F,  calculated  when 

+*Vi  _ 

5  has  been  added  to  the  j  state  of  x 

The  check  gives  an  approximation  to  the  elements  of  A  as  a 
result  of  small  changes  in  the  state  vector,  x.  If  the 
matrix.  A,  derived  numerically  agrees  with  A  evaluated  at 
the  state,  x,  this  provides  assurance  that  the  partial  deri¬ 
vatives  taken  in  deriving  A  are  correct. 

The  H  matrix  was  set  up  as  given  in  Appendix  B  and  was 
verified  in  a  similar  manner  to  the  A  matrix.  The  verifica¬ 
tion  resulted  from  looking  at  the  equation: 

G(x.  +  6  , t)  -  G (x, t) 

K.  =  =^J: - j - - -  (5-3) 

where  Hi  =  the  i*^  column  of  the  H  matrix 
<$  =  same  as  before 

G (x,  t)  =  the  evaluation  of  G,  at  the  state  vector  x 
G(x^  +  5,t)  =  the  evaluation  of  G  with  <5  added  to  the 
i^*1  state 

After  the  partials  in  A  and  H  were  all  shown  as  correct 


a  check  on  the  $  matrix  was  performed.  Again  a  numerical 


I 


check  was  used  to  see  if  was  propagated  in  time  correctly. 

The  matrix  was  propagated  using  eq  2-19: 

±(t,tQ)  =  A(t)$(t,tQ)  (2-19) 

and  the  check  was  accomplished  by  using: 

X(x.  (t  )  +  6,t)  -  X(x(t  )  ,t) 

ii  "  — ; — - 3 - 2 -  (5"4) 

where  =  the  l  column  of  the  $_  matrix 
6  =  same  as  before 

X(x(tQ) ,t)  =  the  state  vector  as  a  function  of  t  and 
initial  conditions,  x(t  ) 

X(Xi(t0)  +  5,t)  =  the  state  vector  as  a  function  of  t 

and  initial  conditions,  x(tQ)  with  6 

4"  Vi 

added  to  tnc  i  state 

With  the  matrices  checked  out  the  next  step  was  to  set 
up  and  check  out  the  filter’s  ability  to  converge  to  a  solu¬ 
tion.  This  was  done  by  setting  up  a  least  squares  filter. 

The  least  squares  algorithm  is  essentially  the  same  as  the 
Bayes,  the  basic  difference  being  that  least  squares  esti¬ 
mation  only  minimizes  the  squares  of  the  measurement  residuals 
and  doesn’t  use  information  from  previous  state  estimates.  So, 
th  e  update  equations  for  the  covariance  and  state  estimate  were 
given  by: 

P(+)  =  (TT(ti)QT1T(ti) ]_1  (5-5) 

x  =  P(+)T(ti)Q71F(ti)  (5-6) 
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Since  the  Bayes  filter  during  initial  update  reverts  to  least 
squares  estimation,  if  no  a  priori  information  is  known,  this 
was  a  good  check  to  see  if  the  Bayes  filter  would  have  any 
problems  with  convergence  to  a  solution  at  epoch. 

The  check  was  accomplished  using  simulated  measurements 
to  a  satellite  in  orbit,  first  with  zero  acceleration  and 
then  with  a  constant  acceleration.  For  ease  of  computation, 
astrodynamic  units  from  Bate,  Mueller  and  White  (ref  1:429) 
were  used  in  all  test  cases.  Five  measurements  at  0.33  time 
unit  (approximately  266  second)  intervals  were  used  in  the 
filter  update.  Each  measurement  consisted  of  an  elevation 
and  an  azimuth  from  both  observers.  The  least  squares  filter 
was  able  to  detect  and  correct  perturbations  in  the  first  six 
states  on  the  unaccelerated  trajectory  as  shewn  in  Table  1. 
With  a  constant  acceleration  of  1/6  g's,  the  filter  was  again 
able  to  converge  to  the  correct  solution  after  perturbations 
in  all  seven  states.  This  is  shown  in  Table  2. 

In  both  cases,  the  least  squares  filter  gave  an  estimate, 
accurate  to  seven  decimal  places  of  the  true  solution,  within 
two  iterations.  Although,  in  each  of  the  cases  the  accelera¬ 
tion  was  constant,  the  results  gave  some  optimism  in  that 
the  estimation  problem  using  the  infrared  sensors  may  be 
plausible.  It  should  be  noted  that  the  time  interval  between 
measurements  was  quite  large.  Time  intervals  of  this  magni¬ 
tude  are  unacceptable  for  estimating  acceleration  during  the 
ascent  stages  of  a  missile  launch.  As  mentioned  before,  a 
large  variation  in  the  acceleration  over  a  data  span  would 


Table  1.  Good  correction  to  first  six  states 


Filter 

.A. 

Xinitial 

. 

Corrections 

1st 

2nd 

3rd 

1 

1.0290466 

.  IE-4 

- . 118312E-4 

. 182258E-5 

.  4215E-11 

2 

.0005 

.  5E-3 

-.  495331E-3 

- . 468180E-5 

. 1958E-10 

3 

-.0006 

- . 6E-3 

. 598687E-3 

. 131304E-5 

.  7479E-10 

4 

-.0005 

-. 5E-3 

.  505352E-3 

- . 533652E-5 

- . 2134E-10 

5 

.9857 

- .  9E-4 

•781742E-4 

. 122667E-4 

- . 5657E-10 

6 

.0005 

.  5E-3 

- .  497214E-3 

- . 278635E-5 

- . 1401E-9 

7 

.0 

.0 

. 164532E-4 

- . 165155E-4 

- .  7145E-10 

Notes:  Corrections  based  on  3  sets  of  5  observations 
Observations  at  0.33  time  unit  (-266  sec) 
intervals 

Unaccelerated  trajectory 


Table  2.  Good  correction  to  all  seven  states 


■ 

Corrections 

1st 

2nd 

3rd 

1 

1 . C290466 

.  IE-4 

- . 130934E-4 

. 308629E-5 

- . 9982E-11 

2 

.0005 

.  5E-3 

-. 485662E-3 

- . 143514E-4 

. 9722E-10 

3 

-.0006 

-.  6E-3 

.  596834E-3 

. 316469E-5 

. 8925E-9 

4 

-.0005 

- . 5E-3 

. 510810E-3 

- . 108001E-4 

- .  4067E-10 

5 

.9857 

- .  SE-4 

. 511362E-4 

. 393112E-4 

- .  3160E-9 

6 

.0005 

.  5E-3 

-.  494899E-3 

- . 509932E-5 

-.1413E-8 

a 

•  .16665 

- . 167E-4 

. 685476E-4 

- . 519284E-4 

.4755E-9 

« 

Notes:  Corrections  based  on  3  sets  of  5  observations 
Observations  at  0.33  time  unit  (=266  sec) 
intervals 

Trajectory  acceleration  constant  1/6  g 


make  the  constant  acceleration  approximation  less  valid. 

Several  data  rates  were  investigated  while  checking  out  the 
Bayes  filter. 

The  Seven  State  Filter 

Knowing  that  the  matrices  and  all  other  subroutines  were 
performing  correctly,  the  computer  program  was  set  up  for  the 
Bayes  algorithm  given  in  Chapter  IV.  The  program  is  presented 
in  Appendix  C. 

The  initial  testing  of  the  filter  used  perfect  data  at 
one  second  intervals.  Five  sets  of  measurements  were  used 
for  each  update.  A  constant  thrust  launch  profile  was  simu¬ 
lated  to  determine  if  the  filter  could  follow  a  target  vehicle 
through  an  entire  trajectory.  With  deviations  in  the  initial 
guess  for  the  first  and  seventh  states,  the  filter  was  able 
to  converge'-  to  a  correct  solution  within  three  iterations  and 
then  estimate  all  the  states  to  within  six  decimal  places 
throughout  the  launch. 

The  next  test  was  to  determine  the  filter's  ability  to 
estimate  a  variable  acceleration  profile.  Because  of  the 
computer  time  necessary  to  run  a  full  launch  profile  through 
the  estimator,  only  the  first  stage  of  the  truth  model  was 
used  while  testing  at  a  one  second  data  rate.  The  filter 
was  able  to  detect  the  changing  acceleration  and  corrected 
towards  the  true  solution.  However,  the  estimates  started 
to  lag,  as  the  filter  started  to  progress  along  the  trajectory. 
The  lag  occurred  in  all  the  states  but  was  especially  notice¬ 
able  in  acceleration.  By  the  end  of  the  profile  the  filter 


was  2.3  g’s  less  than  that  of  the  true  acceleration.  This 
is  shown  in  Figure  7. 

At  first,  it  was  thought  there  was  an  observability 
problem  in  the  filter.  The  covariance  matrix  varied  by 
twelve  orders  magnitude  between  the  corresponding  elements 
for  position  and  acceleration.  However,  the  eigenvalues 
of  the  covariance  matrix  were  positive.  This  indicated  that 
the  matrix  was  positive  definite.  After  further  investiga¬ 
tion,  it  was  found  that  velocity  and  acceleration  elements 
of  the  covariance  had  decreased  by  five  and  eight  orders  of 
magnitude  respectively,  during  the  run.  So,  as  the  covar¬ 
iance  steadily  decreased  towards  zero,  the  filter  put  more 
emphasis  on  the  dynamics  model  and  less  on  the  data. 

To  correct  this  problem,  a  fading  memory  was  added  to 
the  filter  in  which  elements  of  the  covariance  matrix  are 
deweighted  to  reflect  decreased  confidence  in  the  seventh 
state.  This  w  accomplished  by  multiplying  the  inverse 
covariance  mati.  .c,  P“^(tT),  by  a  scalar,  $,  just  after  pro¬ 
pagation.  g  took  on  values  between  zero  and  one,  depending 
upon  the  amount  of  fading  desired.  If  a  value  of  one  was 
used,  the  filter  retained  its  full  expanding  memory.  No 
memory  was  retained  for  a  zero  value  and  the  filter  would 
revert  back  to  a  basic  least  squares  estimator. 

Incorporating  the  fading  memory,  the  filter  demonstrated 
improved  performance.  After  testing  many  values  of  6,  it  was 
found  that  as  the  memory  of  the  filter  was  decreased  the 
estimation  of  the  acceleration  profile  became  better.  In 


Figure  7.  7-state  filter 


fact  with  3  equal  to  zero,  the  filter  gave  a  very  good 
estimation  of  the  acceleration  profile  as  shown  in  Figure 
8.  However,  the  estimates  of  the  position  and  velocity 
were  on  the  high  side.  A  6  of  0.01  gave  the  filter  its 
best  performance  using  perfect  data.  Figure  9  shows  the 
filter’s  estimate  cf  acceleration  using  this  8. 

The  filter  had  demonstrated  the  ability  to  follow  an 
increasing  variable  acceleration  profile.  The  full  launch 
profile  was  given  to  the  filter  next,  to  determine  its 
ability  to  follow  a  staging  event.  This  time  five  measure¬ 
ments  of  perfect  data  at  five  second  intervals  were  used 
for  filter  update.  Again,  sets  of  five  were  used  for  ease 
of  implementation  and  to  limit  computer  time.  It  was  found 
that  the  filter  had  to  have  at  least  three  measurements  in 
order  to  make  all  seven  states  observable.  On  the  other 
side,  if  large  amounts  of  measurements  were  used,  the  filter's 
constant  acceleration  approximation  would  become  less  valid. 

At  first,  it  was  thought  that  with  the  increased  data 
interval,  observability  in  the  filter  might  be  decreased. 
However  the  covariance  matrix  became  better  conditioned. 
Instead  of  the  twelve  orders  of  magnitude  found  with  the 
one  second  data,  it  was  now  only  nine.  The  increase  in  time 
between  measurements  made  it  easier  for  the  filter  to  observe 
changes  in  the  states. 

With  deviations,  in  the  initial  estimate  of  the  first 
and  seventh  states,  the  filter  demonstrated  good  performance 
in  recognizing  the  staging  events  and  correcting  back  to  the 


Figure  8.  7-state  filter  vs.  truth  model:  1  second  perfect  data 


Figure  9.  7-state  filter  vs.  truth  model:  1  second  perfect  data 


true  solution.  Again,  with  limited  fading,  the  filter 
lagged  in  its  estimates,  which  is  evidenced  in  Figure  10. 

By  experimentation,  it  was  found  that  a  3  of  0.06  gave  the 
best  performance  for  the  five  second  measurement  interval 
(Figure  11) .  As  the  data  span  for  an  update  was  increased, 
less  fading  was  required  to  obtain  the  filter's  optimum 
performance.  More  emphasis  had  to  be  put  on  the  dynamics 
model  as  the  constant  acceleration  approximation  became  less 
valid. 

An  initial  goal  was  to  determine  the  filter's  capabi¬ 
lities  using  a  ten  second  data  rate.  With  this  measurement 
interval,  three  measurements  were  required  for  the  update. 

As  before,  a  minimum  of  three  was  needed  t~  make  all  the 
states  observable.  However,  with  more  than  a  thirty  second 
data  span,  the  change  in  acceleration  became  too  great  for 
the  filter  to  handle,  and  excessive  corrections  to  the  states 
caused  the  filter  to  diverge.  Figures  12,  13,  and  14  show 
the  filter's  estimates  of  the  vehicle's  acceleration  profile 
using  various  3's.  A  6  of  0.1  gave  the  best  estimation  of 
the  acceleration  without  significant  degradation  to  the  other 
state  estimates.  Various  initial  estimates  were  also  tried 
in  the  filter.  With  poor  estimates  in  all  the  states,  the 
filter  diverged  quickly.  With  perfect  estimates  in  the  posi¬ 
tion  states  and  degraded  estimates  in  the  other  four  states, 
the  filter  was  able  to  converge  to  a  solution.  This  showed 
favorable  results,  since  a  good  initial  guess  in  position  of 
a  rocket  launch  would  be  achievable  while  velocity  and  accel¬ 
eration  are  less  well  known. 
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Figure  10.  7-state  filter  vs.  truth  model:  5  second  perfect 


Truth  Model 


Figure  11.  7-state  filter  vs.  truth  model:  5  second  perfect  data 
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Figure  12.  7-state  filter  vs.  truth  model:  10  second  perfect  data 


TIME  (MINUTES) 

Figure  13.  7-state  filter  vs.  truth  model:  10  second  perfect  data 


Figure  14.  7-state  filter  vs.  truth  model;  10  second  perfect  data 


The  filter  had  shown  the  ability  to  estimate  the  accel¬ 
eration  profile  of  a  launch  vehicle.  However,  this  had  been 

done  with  the  use  of  perfect  data.  The  measurements  were 

> 

assumed  to  be  unbiased  and  uncorrelated  in  time.  So  zero- 
mean  white  Gaussian  noise  was  added  to  the  measurement  angles 
to  simulate  imperfect  data.  The  standard  deviation  (a)  of 
the  noise  was  steadily  increased  to  determine  when  the  per¬ 
formance  of  the  filter  became  degraded.  There  was  no  serious 
degradation  in  performance  until  a  a  of  1.0  x  10-^  radians. 
This  standard  deviation  equates  to  approximately  150  feet. 
Figure  15  shows  the  filterls  estimate  initially  dropped  off 

but  then  recovered.  At  launch  with  the  vehicle  moving  slowly 
* 

off  the  pad,  the  error  ellipsoids  start  to  overlap  as  more 
corruption  is  introduced  into  the  measurements.  The  filter 
had  trouble  discerning  changes  in  the  states  of  the  vehicle 
at  liftoff.  As  the  vehicle  moved  faster,  the  filter  was 
better  able  to  distinguish  changes  and  correct  back  towards 
the  true  solution. 

Increasing  g  and  expanding  the  filter's  memory  produced 
increased  performance  as  the  noise  was  increased.  With  a  g 
of  0.3,  the  filter  was  able  to  follow  the  vehicle's  accelera¬ 
tion  profile  up  to  a  a  of  1.0  x  10-:::>  (Figure  15).  No  improve 
ment  of  performance  was  noted  with  further  increases  in  3. 

The  highest  noise  level  attained  before  filter  convergence 

*  —5 

could  no  longer  be  achieved  was  with  a  =  5.0  x  1C  (approx¬ 
imately  6800  feet) .  Although  no  information  is  given  about 
the  first  stage  (Figure  17) ,  the  second  and  third  stage 


Truth  Model 


Figure  15.  7- state  filter  vs.  truth  model:  10  second  data 


Figure  l"7.  7-state  filter  vs.  truth  model:  10  second  data 


estimates  give  a  good  approximation  of  the  actual  accelera¬ 
tion  profile.  With  an  optimal  smoother,  these  estimates 

could  possibly  be  made  better.  However,  this  was  not  inves- 

> 

tigated . 

The  Sight  State  Filter 

By  making  the  appropriate  changes  in  the  F  and  G  vectors 
and  the  A  and  H  matrices,  the  eight  state  filter  was  easily 
implemented.  The  filter  was  initially  given  data  with  one 
second  between  observations.  Having  a  better  model  for 
acceleration,  it  was  thought  that  the  eight  state  filter 
could  handle  a  greater  amount  of  data  for  update.  Data  for 
the  entire  first  stage  was  batched  to  the -filter  for  a  least 
squares  estimate.  .The  filter  diverged  within  three  itera¬ 
tions.  Twenty  measurements  were  tried  with  initial  estimate 
deviations  in  the  seventh  and  eighth  state.  The  filter  was 
able  to  converge  to  the  correct  solution  in  two  iterations. 
However,  when  the  time  interval  between  observations  was 
increased  to  ten  seconds,  the  covariance  became  ill-condi¬ 
tioned  and  the  filter  diverged  within  four  iterations  (Table 
3) .  The  eigenvalues  of  the  covariance  matrix  had  a  spread 
of  16  orders  of  magnitude.  Several  other  numbers  of  measure¬ 
ments  were  tried  with  no  improvement  in  the  filter's  observ¬ 
ability.  With  no  significant  results  obtained,  the  attention 
of  the  study  was  directed  towards  the  seven  state  filter. 

Ideally,  the  performance  parameters  of  a  missile  (M  and 
Ve)  would  be  obtained  directly  with  the  eight  state  filter. 

By  running  the  seven  state  filter,  the  data  for  individual 
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stages  could  be  separated.  Then  the  data  could  be  run 
separately  in  the  eight  state  filter-  to  estimate  the  perfor¬ 
mance  parameters  of  each  stage.  Estimates  of  position  and 
velocity  at  staging  obtained  from  the  seven  state  filter 
could  be  used  to  start  the  eight  state  filter.  With  the 
performance  parameter  estimates  of  each  stage,  the  accelera¬ 
tion  profile  could  be  computed  and  compared  with  the  profile 
obtained  from  the  seven  state  filter  and  get  a  better  estimate 
However,  the  eight  state  model,  as  developed  in  the  study, 
did  not  produce  the  desired  results. 


Table  3.  Demonstrated  filter  instability  (8-state) 

Filter 

A 

Ax 

from 

true 

Corrections 

xinitial 

1st 

2nd 

3rd 

4  th 

1 

.1474736 

0 

. 1089E-4 

-. 1777E-3 

.  8439E-2 

-.1134E+13 

2 

-.8695592 

-.001 

. 9290E-3 

. 8686E-4 

.  4222E-1 

. 6660E+13 

3 

.4731659 

0 

. 4499E-4 

-. 4055E-4 

. 2317E-1 

- . 3636E+13 

4 

.0005689 

0 

-.1001E-2 

. 3010E-2 

-. 4045E+0 

. 8746E+14 

5 

-.0033487 

0 

. 5865E-2 

- .  6365E-2 

.  2344E+1 

-. 5128E+15 

6 

.0018243 

0 

- . 3203E-2 

.  3643E-2 

- . 1281E+1 

•2801E+15 

7 

.318 

.0001 

. 3148E+0 

.7562E+1 

- . 1678E+4 

. 5034E+18 

8 

l  ■  ....  ■■ 

3.72 

-.0002 

- . 2930E+1 

-.1195E+2 

.  1969E+4 

-. 5950E+18 

Notes:  Correction  based  on  20  observations 
Observations  at  10  second  intervals 


VI  Conclusions  and  Recommendations 


In  this  study ,  an  inverse  covariance  or  Bayes  filter 
was  developed  to  estimate  the  performance  parameters  of  a 
launch  vehicle.  Two  orbital  observers  with  assumed  angles 
only  (IR)  measurements  were  used  for  filter  update.  A  seven 
state  and  an  eight  state  dynamics  model  were  evaluated  in  the 
filter. 

The  seven  state  filter  modelled  acceleration  as  constant 
and  lagged  in  its  estimates  of  the  launch  profile.  Addition 
of  a  fading  memory  to  the  filter  improved  performanct  signi¬ 
ficantly.  Although  a  Monte  Carlo  analysis  was  not  performed, 
due  to  initial  problems  encountered  during  the  study,  the 
filter  showed  good  performance  while  varying  initial  condi  - 
tions,  data  rates,  trajectories  and  noise  levels.  x^It  achieved 

a  solution,  estimating  a  launch  profile  with  a  ten  second  data 

-5 

rate  and  a  noise  level  of  a  =  5.0  x  10  radians. 

Results  indicate  that  variable  acceleration  cannot  be 
estimated  with  an  eight  state  filter  with  acceleration 
modelled  using  engine  exit  velocity,  launch  vehicle  mass  and 
propellant  mass  flow  rate.  The  observability  became  worse 
with  a  higher  dimensioned  state  vector. 

It  is  recommended  that  further  study  be  directed  towards 
the  seven  state  filter.  Investigation  into  a  fading  memory 
differential  corrector  might  be  warranted.  Using  residue 1 
monitoring  for  adaptive  choice  of  6  might  provide  a  better 
estimate  during  a  staging  event  or  just  after  lift-off.  Also 
an  optimal  smoother  might  be  considered  to  provide  a  better 


estimation  profile  using  noisy  data.  Lastly,  examination  of 
using  alternative  measurements,  such  as  range  or  range  rate, 
is  also  recommended. 

Because  of  failure  of  the  eight  state  filter,  direct 
estimation  of  the  rocket  engine  performance  parameters  was 
not  possible.  Knowing  that: 

a(t)  =  j  _  M(t  _  t  )-  (2-22 


where  a  =  V  M  (initial  acceleration) 
o  e 

a  two  state  filter  could  be  derived  with  aQ  and  M  as  the 
states.  Acceleration  estimates  from  seven  state  filter 
would  be  used  as  the  data  to  estimate  the  two  states  for 
each  r.tage.  With  estimates  of  aQ  and  M,  Ve  can  be  obtained 
directly. 
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Appendix  A 


Derivation  of  the  A  Matrix 

The  elements  of  the  A  matrix  are  found  by  taking  the 
gradient  of  the  dynamics  vector,  F. 

A  =  VF  (A 


where  the  elements  are  given  by 


(A 


For  the  seven  state  filter 
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Using  the  state  equations  in  Chapter  II,  the  nonzero 
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where 


m 


,\v 


i 


and  other  variables  are  given  in  Chapter  II. 

For  the  eight  state  filter,  the  A  matrix  is  8  x  8  with 
six  new  non- zero  terms : 


A  = 


v  a 
x 


47  vV„ 


(A-28) 


A.  „  = 
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vx / a2t  a 
v  V  M  M 


(A-29) 


Appendix  B 


Derivation  of  H.  Matrix 

The  elements  of  the  H  matrix  are  found  by  taking  the 
partial  derivatives  of  elevation  and  azimuth  with  respect 
to  all  elements  of  the  state  vector.  For  the  seven  state 
filter,  H  is  a  2  by  7  matrix  given  by: 


Prom  eqs  (B-10)  and  (B-ll) 


-R(r  •  R) 

A 

+  r  •  k 

Jr 

[rJ  _ 

-R(r  •  R)  .  — 

- i +  r 

!r|  !r| 

the  numerator  is: 

-v*v*v*v 

2  2  2  +  Z 

ir+ir+ir 

x  y  z 

Let:  DOT  =  xRx+yRy+zRz 

RSQD  =  Rx+Ry+Rz 

So  the  numerator  can  be  written:  z  -  Rz  DOT 

RSQD 


RxDOT^  I  RyDOT^  f  RzDOT^ 
"RSQD“ j  +  [y~RSQD~]  +  Z~RSQD  j 


(B-l 


The  sign  of  eqs  B-15,  B-16,  and  B-17  was  determined  by  looking 
at  the  z  component  of  the  cross  product  R  x  r.  If  the  z  com¬ 
ponent  was  negative  the  sign  was  negative. 

For  the  eight  state  filter  H  became  a  2  x  8  matrix  with 
the  added  elements  being  zero. 


Appendix  C 

Bayes  Filter  (Seven  State)  Program 


PROGRAM  BAYESdNPUT,  OUTPUT*  rAPE7) 

EXTERNAL  F 

REAL  XREF(7),DELTA,PHI(7,7),Y(56)  »T I  ME , ELMAT , AZM AT, ELMAT 2 , 
AZMAT2,T,T0UT,RELERR,A8SERR,W0RK(1276),IW0RK(5),GMA7(2,1), 
G.-^ATZ  (2  » 1  )  »  XO  BS  (3  )  ,X0BS2(3> ,HMAT(4»  7) »HMATi  (2, 7) , HMAT2 ( 2  ,7 > 
H  (  4  *7  )  ,H73ANS(7,4),Q(4,4)»QI N  V  (  4  *  4  ) »=1 ( 4 , 7 ) ,PI NV ( 7,7 ) , D 1 ,02 
D£LX(7,1) ,81 (4,1 >,B2C7tl ) » WK AR E A , Qtf , QZ » 3 V, PZ* TMAT (7 ,7) , 
XNEU(  7),H‘!QR(7,1),R(4»1)*XHNS(7)»XDIFF(7,1)»PMN3I  (7,7), 
S’R(7,1)  ,PX(7»1>,PPLS(7,7>,PP(7,7),PMNS(7,7),PHIT(7»7) , 
HTQHC  7,7) , BEY  A (7, 7) ,3MNS IC ( 7 ,7 ) *XPL OT (1 22 ) , YPLOT( 1 22) , 

EA,EP ,EV,TT(611) ,X(611> ,YY(611)*Z(S11> ,VX(S11) ,VY(611 ), 

VZ (611), A (611 ) ,EX2,EY2,EZ2*EVX2,EVY2,EVZ2,EP2,EV2,EA2, 

RMSP,  RMStf,RMSA»yGRK2(7)  ,P3IV<7,7)  ,ZZ(7) 

INTEGER  COUNT  •MUM*  I  ,  J,I  DGT  ,  I ER  ,  T I ,  CODE,  I  TER  ,  ITER  2 
DIMENSION  QVilO ) ,QZ(1Q) ,PV(28) ,PZ(2  3  ),WXAREA(25C ) , 

TIME( 0J161) »ELMA7(163 ) , AZMAT( 1 60 ) , ELMAT2 ( 160 ) , AZMAT2 (1 60 ) 
PARAMETER  (  C0UN7=61> 

.. .INITIALIZE... 

XREF(1)=. 71009116 
XREF(2)=-. 41327566 
XR£F(3>=. 56640624 

XREF(4 )= 100 *COS(-. 53249995 )*COS(C. 60 21 33 593/25936.24 764 
XREF( 5 )  =  1 30 *~IN(-. 53249995 >*COS (0.60 2139 59) /25936. 24764 
XR £F(6)=10G*$IN(3.6Q21 33 59 )/2 59 36.24754 
XREF (7  )=1.184 
71=0 

TIME(3)=C.O 

ITER=1 

ITER2=3 

X=C 

ICODE=l 
M= 1 

E3  =G • S 
EV=0 .0 
EA=C • 0 
EP2=G.O 
EV2=C.O 
EA2=C.O 
MEAN =3 .3 
S7D=0.G 

CALL  RAN  3  ETC  77) 

DO  2  1=1,7 
DO  2  J=l,7 

xmns(  :>=xref(d 

PM  NS I( Z  « J) =0.0 
BETAd  ,J)=0.G 
CONTINUE 


BETAd, 1)=0.1 
BET  A(2,2)=C.l 
BETA ( 3 ,3 ) =0 .1 
BET  A ( 4 ,4 )=0.i 
BETA(5,5  >=0.1 
8ETA (6,6  )  =  C.l 


BE^A (7, 7)  =0.1 
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DO  5  1=1,56 

yn>=o.o 
CONTINUE 
DO  1C  1=1,4 
DO  10  J=l»4 
Q< I, J)=0.0 
CONTINUE 
DG  15  1=1,4 

Qd  *1  )=1.’JE-14 
CONTINUE 

-•.FIND  Q  IN  VL  RSE... 

CALL  CHNGV(Q,4,QV) 

CALL  L IN V2P( QV,4, QZ, I06T , D1 ,D2, WKAREA ,IER > 

CALL  CHN  GM( Q2 , 4, QIMV ) 

...CEAD  IN  DATA  AND  ADD  NOISE**. 

DO  2“  NUH=1, COUNT 
READ  (5,  *)TIME(wUM)  ,ELMAT(NUM)  *  AZHAT  (NLJN  >  ,ELMA72  (NUN  ?  » AZMAT2!N 
EL MAT!  NJH)=ELMAT  C  NUH>+ GAUSS ( MEAN , STD ) 

AZ WAT < NU M> =AZKAT (MJ*> ♦GAUSS! MEAN, STO> 

ELMAT2CN UM) =ELMAT2  CNUM) ♦GAUSS ( MEAN » 3 TD ) 

AZMAT2 (NUM)=AZMAT2{NUW)+GAUSSCMEAN,STD) 

CONTINUE 
DC  200  1=1,610 

sEADC4,*>x<i),yy<i>  ,zci),vxci),vym,vz(i>  ,A(i> 

CONTINUE 

PRINT*,*  TIME=Q.G  * 

...COMPUTE  RESIDUALS  AND  H  MATRICES... 

DO  25  1=1,7 
DC  25  J=l,7 

HTQH(I,J)=C.O 
H*QRCI,11  =  C  .0 
PHICI,  J)=0.0 
XNEWd  )  =  XREF(I) 

CONTINUE 
DO  27  1=1,7 

°HHI,I)=1.0 
CONTINUE 

DC  50  MJM=ITER,ITER2 
T=TIM£  (N'.'M-l  ) 

TC*UT=  T I  HE  (NUM  ) 

RELER  ?  =  1 .CE-D  7 
ABSER - =1 • OE-O  7 
\E9N=  5  6 
IFLAG=-1 
DO  30  1=1,7 
DO  3C  J=1 ,7 

yc  :)=xne«( : > 

Y (  I  *7  +  J)=PHKI  ,J) 

ccnt: nue 

CALL  ODE (F,NEQN,Y,T,TOUT,RELERR,ABSERR,IPLAG,WORK,iyCRK) 

DO  35  1=1,7 
D:  35  J=1 ,7 

PHI (I,J)=Y  C I*7  +  J) 

X'lEWd  )  =  Y  C  I  ) 


■\  * 


35  CONTINUE 

*  •••FIND  POSITION  OF  OBSERVERS  AND  CALCULATE  G  **ATR  IX  AND 

v  RESIDUALS ... 

CALL  EL'l  AZCTIHEtNUM)  , XNE U ,GMAT . SMAT 2 . X 59 3 » X03S2 ) 

F<1»1)=EL«ATCNUM)-GHAT(1,1I 

PI2,1 )=AZMAT (NUM)-GMAT (2,1  > 

S{3,1>=EIMAT2<NUM>-GMAT2<1,1> 

R (4*1 )=AZMAT2(NUM)-GMAT2(2*1 > 

*  ...FINO  H  MATRICES..* 

CALL  MA7H(XNEW<1 1 ,XNEM(2> ,XNEU(3),X3BS(1 >»XQBSC 2>  *X0BS(3) ,HMA 
CALL  MAT  H( XNEU (1 > , XNEU (2 ) , XNEU<3 > , X 38 S2<1 >  »X08S2 C 2 > , XOBS2 C 3 > , 
♦  HMAT2  ) 

DO  40  J=l,7 

HMAT(l,J>=HMArl(l,J) 

HMAT<2»J)=HHAT1 (2«J) 

HMAT(  3»0)=HMA'-2<1  »J) 

HMAT(4,J)=HMA72(2,d> 

40  CONTINUE 

*  ...MULTIPLY  H  BY  PHI... 

CALL  M*PY(HMAT,4»7,PHI  »7,H  > 

*  ...FIND  H  TRANSPOSE... 

DO  45  1=1,4 

DC  45  J-1,7 

HT  R  AN3 ( J  »I  )  =H  ( I  »<J) 

45  CONTINUE 

*  ...FIND  HTRANS*Q  INV£R$E*H... 

CALL  HMPY(QINV,4,4,H,7,P1> 

CALL  MMPY(HTPANS,7,4»P1»7,TMAT> 

DO  55  1=1.7 

D"  55  J=1 . 7 

HTQHU  ,J)=HTQH(I,J>+TMA7CI,J> 

55  CONTINUE 

*  ...FIND  H  TRANSPOSED  INVERSE** 

CALL  MMaY(QINV»4«4*R»l»Bl) 

CALL  MMP  Y (HTR  AN3 .7.4«Bi»l»B2) 

DO  60  1=1*7 

HTQRCI  .1  J=HT Qfi  (I , 1 > *B2 < I ,1 > 

60  CONTINUE 

50  CONTINUE 

DO  65  1=1.56 
Y(I)=C.O 
65  CONTINUE 

K  =  K  + 1 

*  ...FIND  P  PLUS... 

DC  70  1=1,7 

DO  75  J=1 ,7 

PI NVC I . J)  =  PMNSI (I  ,J>«-HTQH(I  ,J) 

70  CONTINUE 

CALL  CHNGV(PINV,7,PV> 

CALL  LINV2P(PV,7.PZ,:DGT,D1,D2,WKAREA,IER ) 

CALL  CHNGM?°Z»7.PPLS> 

*  ... COMPUTE  EIGENVALUES  OF  UPDATED  COVARIANCE... 

IF  (K.EQ .1 )  THEN 
DO  72  1=1,7 
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00  72  J=l,7 

PSIV(I,J)=PPL5(I  ,J) 

CONTINUE 

CALL  EISPS(PSIV,7*10  »ZZ»PV*7*U0RK2»IER) 

PRINT*,*  EIGENVALUES  • 

DO  4C0  1=1*7 

PP  INF  *  »  •  *,ZZ(I> 

400  CONTINUE 
END  IF 

*  ...COMPUTE  UPOATE  TO  STATE  VECTOR... 

DO  75  1=1,7 

XDIFF (I *1 )=XMNS (I )— XREF< I) 

75  CONTINUE 

CALL  HMPY(PMNSI*7,7,XDIFF,1,3TR) 

DO  20  1=1,7 

PX(I, 1)=$TR(I ,1)+HTQR(I*1) 

80  CONTINUE 

CALL  MMPY (PPLS»7 » 7»PX, 1 » DELX ) 

DO  5  5  1=1,7 

XREF( :>=XREF(I)+DELXCI,1) 

25  CONTINUE 

IF  CK.EQ.l)  'HEN 
PR  IN' *,*?(♦)  =  * 

PRINT* (7 (7(1 X, El  5.6)/))*  * C CPPLS C I , J) , J  =  1 , 7 ) , I =1 ,7) 

r,  END  IF 

XW  PRIN7*(7(1X»E15.3))*, (0ELX(I»1),I=1,7) 

PRINT*,* XREF=  * 

PRINT*  (7  (1X,E15.  •))/>*,  (XREF(I),I=1,7) 

IF  (K.EQ.3)  GO  TO  86 

*  ...CHECK  FOR  CONVERGENCE... 

IF  (A8S(DELX(1,1>>.G7.PPLS(1»1>>  GO  TO  22 

IF  (ABS(DELX(2,i>>.GT.PPLS(2,2)>  SO  TO  22 

IF  (ABS( DELX(3,1)).GT.PPLS(3,3))  GO  TO  22 

IF  (ABSJ 0ELX(4,1 ) ).GT.PPLS( 4,4) )  GO  TO  22 

IF  (ABS( DELX (5,1) ).GT.PPLS(5,5))  GO  TO  22 

IF  (ABS(DFLX(6,1)).GT.PPL5(6,6>)  SO  TO  22 

IF  (ABS(DELX(7,1)).GT.PPLS(7,7))  GO  TO  22 

86  XPLCT(M)=(TIHE(TI)*0 .00 30 986)*80 6.3 l 1 5 74 4 
YPL0T(M)=XREF(7) 

WRITE  (7  ,110  )XPLOT(M)  ,vpi_OT(«> 

110  FORMAT (Fe.4*2X*E15. 3 > 

£X2  =  (XREF(1 ) -X ( I  CODE) ) **2 

*  ...COMPUTE  OEVIATIGNS  IN  STATES  FROM  TRUTH  MODEL... 
EY2= ( XP£F (2 )-YY( I  CODE) >**2 
EZ2=(XPEF(3)-Z(IC0D£)>**2 

EVX2=(XREF(4) -VX (IC00E))**2 
EVY2=(XR EF(5)~VY (IC0DE))**2 
EVZ2  =  ( XR  EF(6 )-VZ ( I CODE) ) *  *2 
EA=(XREF  (7)-A(IC:.DE)  >**2 
EP= EX2+EY2+EZ2 

V?  EV  =  EVX2«-EVY2*EVZ2 

EP2=FP2+‘E° 

EV2=EV2*EV 
EA2=EA2*F  A 
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*  ..•PROPAGATE  STATE  AND  COVARIANCE... 

T=TIME<“  II 

TI=r 1*1 
TOUT=TIM£(TI) 

RELERR=1 .0E-07 
ABS£RR=l.CE-07 
NEQKs56 
IFLAG=-1 

DC  87  1=1,7 

YCI )=X3EF( I) 

Y(  I*8)=1.Q 
57  CONTINUE 

CALL  ODE  (F,N}:QN,Y,T,TOUr,RELERR,A8S*  RR,  I  FLAG,  WORK,  I  WORK) 
DC-  90  1=1*7 
DC  93  J=l,7 

XR£F(I)  =  Y(I) 

XNEUCI )=Y(I ) 

XMNSC I )— Y( I > 

PHI (I,J)=Y( I*7*J) 

PHIT(J,I)=YCI*7+J> 

90  CONTINUE 

*  ...FIND  P  MINUS  AND  P  MINUS  INVERSE... 

CALL  MHP Y(PPLS,7 , 7,PHIT»7,PP) 

CALL  MMPYCPHI ,7,7,PP,7,?MNS) 

CALL  CHNGV(PMNS»7,PV) 

CALL  L IN V2PCPV, 7 *PZ» I OGT ,D1 ,02 , VKAREA *IER » 

CALL  CHNGM(P2,7,PMNSI0) 

*  ...MULTIPLY  P  MINUS  INVERSE  BY  BETA... 

CALL  MMPY<PM\SIQ,7»7,8ETA,7,PMNiI> 

PRINT*  C  *  *  TI«E=  •*,F7.5>,*TIM£<TI> 

PRINT*,*  X  MINUS=  » 

PRINT*C7(1X,E15.2  >>*,« XNEUC I) *1=1,7) 

ITER=ITEP+1 

IF  (ITER -GE. COUNT)  THEN 
GO  TO  100 
END  IF 

ITER2  =  I*  ER2  +  1 
IF  CITER2.GT. COUNT)  THEN 
ITER2  =C  OUNT 
END  IF 

ICODE=IC  CDE+IO 

K=0 

M=M+l 

DC  35  1=1,56 
YCI  )=0.0 
55  CONTINUE 

60  TO  22 

*  ...COMPUTE  PMS  ERRORS  OF  FILTER... 

100  RMSP=$QR 7 (EP2/CM-1) ) 

RMSV=SQ,R  T  (EV2/CM-1)  ) 

RMSA=SQR  TCEA2/CM-1)) 

PRINT* (*  *  RMS  ERROR  POSITION  *  *  ,  El  5 . 3  )  *  ,  RMSP 
P°  INI  * ( *  *  RMS  ERROR  VEL3CITY=  *  * ,E15 . 3 ) * ,R MSV 
PR I\T* (*  *  RMS  ERROR  ACCELERATIONS  * » ,E15.5 ) » rRMSA 
END  67 


SU8RCUTI hZ  M«PY( A*IA*JA»B*J8*C) 

*  ...MATRIX  MULTIPLICATION  ROUTINE... 

REAL  Ad  A*  JA>  ,8(  JA,  JB>  ,C(IA,JB> 
DOUBLE  PRECISION  TD 
DO  2G  K=1*IA 
DO  20  J=1»J8 
TD=O.DCO 
DO  10  1=1  *  *1 A 

10  TD=TD+A(K,I >*8f I *J) 

20  C(K,J>=TD 

END 


SUBROUTINE  F(T,Y*YP> 

*  ...SETS  UP  STATES  AND  PHI  MATRIX  FOR  I  NT EGRAT I  ON. • • 

REAL  Y(56),YP(56> »PHI ( 7, 7 > ,FMAT ( 7» 1 > , AMA T ( 7, 7 > ,PHI DOT (7 , 7) , 
CALL  MATF(Y(i>,Y(2>*Y(3> ,Y(4>  *Y(5>  *Y(5>»Y(7>,FMA7) 

DO  1C  1=1*7 

YP(I)=FMAT(I,1> 

10  CONTINUE 

CALL  MA’’ACY(l)fY(2)*Y<3),Y<4)*YC5)*Y(S)fYC7).AMAT) 

DO  20  1=1*7 
DC  20  J=1 *  7 

PHI (I,J>=Y(I*7*J) 

20  CGNTINUE 

CALL  MM? Y ( A  MAT *7  *7*PHI *7, PH I DOT) 

DO  3G  1=1*7 
DC  30  J=1 *7 

YPCI*7+JI  =  PHID0T(It  J) 

33  CONTINUE 

END 


SUBROUTINE  MATF( X*Y*Z*VX* VY*VZ,A,FMAT> 

*  ...COMPUTES  F  VECTOR 

REAL  FMA  T(7,l >,X,Y,Z»VX,VY,VZ,A,VE,M*T,R3 
R3=(X* *2  2+Z**2)*saRT(X**2+Y**2+Z**2) 

FMATC1*1 >=VX 
FMAT  C  2  *1  )  =  VY 
FMAT (3*1 )=VZ 

FMAT (4*1 )=-X/R3^A*VX/SQRT( VX**2 ♦VY**2* VZ* *2 > 
FMAT  (5*1  >=-Y/R3+A*VY/SQRT(VX**2+VY**2«-VZ**2> 
FMAT  (5,1  >  =  -Z/R3+A*VZ/SQRT(VX**2*VY**2«-VZ**2) 
FMAT (7  ,1  1=0. C 
END 


SUBROUTINE  MATA(X*Y»Z» VX»VY*VZ*A»AHAT) 
...COMPUTES  A  MATRIX... 

PEAL  X»YtZ» VXtVYtVZ*A*R3tR5fV3»A«AT<7,7>, VI 
INTEGE7  I»J 

P3=<X**2+Y**2*Z**2)*3QRT{X**2+Y**2+Z**2> 
R5=((X**2  +  Y*  *2*1* *2 )**2) *3QPT ( X**2*Y ** 2* Z* *2 > 
Vl  =  $GRT<  VX**2-*-VY**2  +  VZ**2  > 

V3=CVX**2+VY**2+VZ**2>*3QRT<  VX**2  +  VY**2«- VZ**2) 
DO  1C  1= 1 v  7 
DC  23  J-l »7 

AMAT(I*J)=G.O 
CCNTI MUE 
CONTINUE 
AMAT  < 1  * A )  =  1 
AMAT  <  2  » 5 >  =  1 
AMAT ( 3  »S )=1 

AMAT  C4  *1 )  =  -1/R3+3*X**2/R5 
AM  AT  C  4  *2  )  =  3*X*Y/R5 
AMAT  C 4  *3 )=  3*X*Z/R5 
AMAT (4*4 )  =  A/V1~VX**2*A/V3 
AMAT(4,5)=  - VX  *V  Y*A/V3 
AMATC4»&)=  -VX*VZ*A/V3 
AMAT<4,7)=  VX/V1 
AMAT (5*1 >=  3*X*Y/R5 
AMAT(5»2)=  -1/R3+3*Y**2/R5 
AMAT (5*3 )=  3*Y*Z/R5 
AMATC5.4>  =  -VX*VY*A/V3 
AMAT(5,5)=  A/V1-VY**2*A/V3 
AMA7<5»&>=  -VY*VZ*A/V3 
AHAT(5»7)=  VY/V1 
AMAT (6*1  )  =  3*X*Z/R5 
AMA7<6»2)=  3*Y*Z/R5 
AMAT (6*3)  =  -1/R3+3*Z**2/R5 
AMAT (6*4 )  =  - VX*VZ* A/V3 
AMAT  (6*5)=  -VY*VZ*A/V3 
AMAT ( 6  *5 )  =  A/V1-VZ**2*A/V3 
AMAT(&*7>=  VZ/V1 
END 


SUBROUTILE  MATG<  X, Y» Z» RSUBX, RSUBY, RSUB? , 5MAT ) 

...COMPUTES  G  MATRIX... 

PEAL  X»Y»Z* RSUBX?  RS-UBY  *P SU8Z  ? EL* AZ  t  DI FF*  DO T* R SQD ?NUM  *NUM1  * 

♦  CROSS *GMA' (2*1 ) 

FSQ0=RSUBX**2+RSUBY+*2+RSUBZ**2 
CROSS  =  RS  UBX*  Y-RSUBY  *  X 
DOT=X*PSUBX*Y*RSUBY+Z*PSUBZ 

NUM=SQRr ((X-RSUBX*DOT/RSQD)**2+(Y-R3U8Y*D0T/RSQD)**2+ 

♦  (Z-RSUBZ*DCT/R3QD)**2> 


DI  FF=SQR  T  (  C  X-*RSUBX  )  **2+(Y-RSUBY)**2*-  ( Z-RSUBZ  )  *  *  2) 
GHAT  (1  »1  >=ASIN(NUM/OIFF) 

NUM1=Z-.R SUBZ* DOT /RSQD 
GHAT  (2*1  )  =  ACOS(NUMl/NUM) 

IF  CCRGSS  .LT •  0.0)  "HEN 
GHAT  <2*1 )=-GMAT(2,l) 

END  IF 
END 


SUBROUTINE  MATH(X,Y,Z,RSUBX,RSU8Y,RSU37*HMAT) 

*  ...COMPUTES  OBSERVATION  RELATION  MATRIX  H... 

PEAL  HMAT  (2, 7) *X* Y, Z ,RSUB X* RS UBY , RSUBZ, R $ QO , NUM, DIFF, DOT 
INTEGER  I*J 
DC  10  1=1,2 
DC  23  J=4,7 

HMAT ( I »J)=3 .0 
20  CONTINUE 

10  CONTINUE 

RSQD=RSU8X**2+RSUBY»*2+RSU3Z**2 

D0T=X*R3U8X+Y*RSUBY*Z*RSUBZ 

MJM=$QRT  ((X-RSUBX*DOt /R3QD)**2+( Y-.RSUB Y* DO  T/RSQD)  **2  + (Z-RSUBZ 

♦  *D0T/RSQD>**2> 

DIFF=SQRT((  X-RSU8X  )*  *2  +  (  Y-RSU8Y)  **2  (Z-RSUBZ )**2) 
if?  CR0SS=R$UBX*Y-RSU6Y*X 

HMAT (1,1 )=((NUM) *( RSUBX-X ) /DIFF**3 ♦  ( X  * (RSUBY** 2+RSUBZ **2 > 

+  -PSU8X*D0T*{RSUBY**2*RSUBZ**2)/RSQ3-Y*RSUBX*RSU8Y+D07*RSU8X 

♦  *  PSUB  Y**2/RSQD-Z*  RSU3X*RSUBZ+D3T  *RS  UBX*P.SUBZ**2  /RSQD)  /DIFF 
/NUM/  R  SQD )/SQRT(l-( NUM/DIFF) ) 

HMAT (1 ,2 )=((NUM)*(RSUBY-Y)/DIFF**3+(D0r*R3UBX**2*RSUBY/RSQD 

♦  -X*RSUBX*KSUBY-DOT*RSUBY*(RSUBX**2*R3UBZ**2)/PSQD 

♦  ♦Y*(RSUBX**2+RSUBZ**2) ♦DOT  *RSUBY  *RSUBZ**2/RSQD 
-Z*RSUBY*RSU8Z)/D:FF/NUK/RSQD)/SQRTC1-(NUM/DIFF)**2) 

HMAr (1,3 )  =  ((NUM)MRSUBZ-Z)/DIFF**3  +  (D0r*RSUBX**2*RSU8Z/PSQD 

♦  -X*RSUBX*fcSUBZ-*-DOT*PSUBY**2*RSUBZ/RSQD-Y*RSU8Y*RSUBZ 

♦  -DOT* RSUBZ*  (RSUBX**2*RSU8Y**2)/RSQD*-Z*'(RSUBX**2+PSUBY**2)) 

♦  /DIFF/NUH/RSQD)/SQRT<l-(NUM/DIFP)**2) 

HMAT (2,1 )=(-l)/SQRT(l-((Z-RSU8Z*D0T/RSQD)/NUM)**2) 

♦  *(( (- RSUBX*RSUBZ/C $  QD  > /NUM )-( (?-RSU3Z*DQT/RSQD)/NUM**3 

*<  (  X-RSUBX*DOT/RSQD)*  (  (  RSUBY **2  *R  S  J  3 Z *  *  2  ) /R SQO  )-(  Y-RSUB Y*DC'" 
+  /RSQD  ) *(RSUBX* RSUBY/.RSQD  )-(Z-R3U8ZOOT/RSQD) 

*  (RSU8X*RSU8Z/ RSQD)  )  )  ) 

HMAT (2,2 )=(-l)/SQRT(l-((Z-PSUBZ*D0T/RSQD)/NUM)**2 ) 

♦  *(( (-RSUBY*RSUBZ/RSQD)/NUM)-((Z-RSU3Z*DOT/RSQD)/NUM**3 

♦  *((  X-RSUBX*DOT/RS‘QD)*(-RSUBX*R$UBY/RSQD)  +  {Y-P$UBY*DOT/RSQO)* 

♦  ((RSU8X**2+R$UBZ**2)/RSQD)-(Z-RSUBZ*D0T/RSQD) 

+  * ( R  SUB Y  *P  SUBZ /RSQD ) ) ) ) 
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HMAT  (  2»3  )  =  <-!  > /S  QP T  (1  -  (  (  Z-RSl'BZ* OOT/ RS  QD  > /N'JM  )  *  *2  > 

►  *<<<l-PSU8Z**2/RSQD)/NUM>-<  <Z-?.SUBZ*00T/R3QD>/NUM*  *3 

*•  *<  (  X-RSUBX*OOT/PSQOi*(-RSUBX*RSUBZ/RSQD>-<  Y-RSU8Y  *DOT/RS  QD  > 

►  *<RSU8Y*RSUBZ/R$QD)+(Z“R$U8Z*DQT./RSQ0)*((RSUBX**2-»'RSUBY**2} 

►  /  F.  S  QO  )  )  )  ) 

IF  (CROSS  .LT.  0.0*  'HEN 
HMAT(2,1>=-HMAT<2,1> 

HMAT(2,2) =-HMA  T (2,2) 

HMAT ( 2  »3)=-HMAT (2  »3 ) 

END  IF 
END 


SUBROUTINE  ELNAZ (TI ME » XR , GH AT , SMAT 2 , XO 3S , X08S2 > 

...CALCULATES  OBSERVERS*  POSITION  FOR  3  MATRIX  COMPUTATI ON.. • 
FEAL  TI ME*XR (7) « GMA T (2  »1 ) »GMAT2(2»1 ) « XO 3S ( 3 ) » X08S2( 3 > 

X0BS(1)  =  6.6*C0S< ( 2* 3. 1 41 5? 2654*^ I  ME )/l 06. 53565) 
X08S(2)=6.6*:iN(  (2*3.1415'?2654*TIME)/I3S.535=>5) 

X0BS(3)=  0.0 

X0BS2(1)=6.6*SIN( (  2*  3 . 141 59  2654 *TI  ME >  / 10  6. 5359  5 ) 

X0BS2(2) =-6.6*C0S ( < 2 *3 • I 41592654* TI ME ) fl 0 6.5353 5 ) 

XGBS2 ( 3) =0 .0 

CALL  MATG(XRa),XR(2),XR<3),XCBS(I)*X03S(2)*X0BS(3),GMAT) 

CALL  MATGtXRCl )  ,  XR (2 > , XR C 3 ) »X0BS2 ( 1 > , X 0BS2 (2 >  ,XC8S2(3>  tGMAT2) 
END 


SUBROUTINE  CHNGV ( AMAT»N? A  VEC~ ) 

...CHANGES  MATRIX  INTO  VECTOR  FORM  r0R  INVERSE  ROUTINE... 
REAL  AHA  T(N,N),AVEC7 ( N*C N+l )/2 ) 

INTEGEP  \»J*I,ICiNT 
ICNT=Q 
DO  10  1=1, H 
DC  10  J=1,I 
ICNT  =1  CN**+  1 

A VECT( I CNT ) =AMAT (IjJ) 

CONTINUE 


* 


10 


SUBROUTINE  CHNlGM  (  A»N  »NMAT)  *  ' 

...CHANGES  VECTOR  BACK  INTO  MATRIX  AFTER  INVERSION.. 
REAL  ACN*<N*l)/2> #NMATCN«N) 

INTEGER  N  »ICNT  *1  *  J 
ICNT=0 
DC  10  1=1* N 
I  CM  T=  I CNT  +1 
DC  10  J=1*I 

NM  AT ( I »J)  =  A  < ICNT ♦J-I  ) 

NMAT<d*I >=ACICNT+J-I> 

CONTINUE 

END 


REAL  FUNCTION  GAUSS < MEAN *STD > 
*  ...RANDOM  NOISE  GENERATOR... 

REAL  MEAN 
SUM=-5. 

DO  1  1=1 *12 
1  SUM=5UM+RANF(> 

GAUSS=S7D*SU*+MEAN 

END 
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